// Include files.
#include <cmath>
#include "DeadzoneFilter.hpp"

/**
 *  Constructor for DeadzoneFilter.
 *  
 *  @param deadzoneRadius The desired radius of the deadzone.
 */
DeadzoneFilter::DeadzoneFilter(float deadzoneRadius)
{
	m_deadzoneRadius = deadzoneRadius;
}

/**
 *  Destructor for DeadzoneFilter.
 */
DeadzoneFilter::~DeadzoneFilter()
{
}

/**
 *  Apply the filter to an input.
 *  
 *  If the input is within the deadzone area, a zeroed JoystickValue will be returned.
 *  If the input is outside the deadzone area, a copy of the input will be returned.
 *  
 *  @param input The input JoystickValue.
 */
JoystickValue DeadzoneFilter::Apply(const JoystickValue &input) const
{
	float distance = sqrt(pow(input.GetX(), 2) + pow(input.GetY(), 2));
	
	if (distance <= m_deadzoneRadius)
		return JoystickValue(0, 0);
	else
		return input;
}

/**
 *  Apply the filter to an input.
 *  
 *  If the input is within the deadzone area, a zeroed TwoJoystickValue will be returned.
 *  If the input is outside the deadzone area, a copy of the input will be returned.
 *  
 *  @param input The input JoystickValue.
 */
TwoJoystickValue DeadzoneFilter::Apply(const TwoJoystickValue &input) const
{
	return TwoJoystickValue(Apply(input.GetLeftValue()), Apply(input.GetRightValue()));
}
